
/*
The same as picinbono-material.cc -- however, for small
displacements, a formulation in terms of U and E is more numerically
more precise.

*/

/*
2 W(I1, I2) =

( - mu - 3/2 lambda) I1 + (lambda/4 + mu/2) I1^2 - mu I2

2 DW = A : DC

where

A = ( - mu - 3/2 lambda) I + lambda/2 I1 I + mu C 
= ( - mu - 3/2 lambda + lambda/2 I1 ) I + mu C 
so

forces = - F A  X^-T

and

Dforces = - (DF A + F DA) X^-T

where

DA = lambda/2 tr(DC) I + mu DC

if we take E = 1/2 (C -I), then we get 

S = 2 D_C(W) = lambda tr(E) + 2 mu E



*/

#include <cmath>

#include "TetrahedronState.h"
#include "Matrix.h"
#include "Mechanics.h"

/*
(+ 
Matrix3::multiply_mm
Matrix3::add_diag
Matrix3::square
Matrix3::trace 
Matrix3::diag
4
Matrix3::saxpy
(* 2 Matrix3::multiply_mm)
Matrix3::negate)

235
*/
int Picinbono2ElasticForce (TetrahedronState const*me,
						  Matrix3 *force_mat,
						  Matrix3 const &disp_mat)
{
	Matrix3 C;
	Matrix3 F;

	Matrix3::MultiplyMM (F, disp_mat, me->inverse_location_);
	F.AddDiag (1.0);

	Matrix3::Square (C, F);

	Real I1 = C.Trace ();

	Matrix3 A;
	A.Diag (- lame_mu +  lame_lambda* (0.5 *I1 -1.5));

	Matrix3::Saxpy (A, lame_mu, C, A);

	*force_mat = F * A * me->invloc_transpose_;
	Matrix3::Scale (*force_mat , -1, *force_mat);

	return 235;
}

/*
(+ 
Matrix3::multiply_mm
Matrix3::add_diag
Matrix3::square
Matrix3::trace 
Matrix3::diag
4
Matrix3::saxpy

Matrix3::multiply_mm 
Matrix3::multiply_mtm 
Matrix3::double_symmetrify 
Matrix3::diag
Matrix3::saxpy

(* 3 Matrix3::multiply_mtm)
Matrix3::add

)

return 421
*/

int Picinbono2ForceDerivative (TetrahedronState const*me,
							 Matrix3 *dforce,
							 Matrix3 *force,
							 Matrix3 const &disp_mat,
							 Matrix3 const & dir_mat)
{
	Matrix3 C;
	Matrix3 F;

	Matrix3::MultiplyMM (F, disp_mat, me->inverse_location_);
	F.AddDiag (1.0);

	Matrix3::Square (C, F);

	Real I1 = C.Trace ();

	Matrix3 A;
	A.Diag (- lame_mu + lame_lambda* (0.5 * I1 - 1.5));

	Matrix3::Saxpy (A, lame_mu, C, A);

	Matrix3 DF;
	Matrix3::MultiplyMM(DF, dir_mat, me->inverse_location_);

	Matrix3 DC;
	Matrix3::MultiplyMTM (DC, F, DF);

	Matrix3::DoubleSymmetrify (DC, DC);	

	Matrix3 DA;

	DA.Diag(lame_lambda * 0.5 * DC.Trace());
	Matrix3::Saxpy (DA, lame_mu, DC, DA);

	*dforce = (DF * A + F * DA) * me->invloc_transpose_;
	Matrix3::Negate (  *dforce, *dforce);
	int flops = 427;
	if (force)
	{

		*force = F * A * me->invloc_transpose_;
		Matrix3::Scale (*force , -1, *force);

		flops += 117;
	}

	return flops;  
}
